﻿using OnDash.Mathematics.Data;
using OnDash.Mathematics.Vector;

namespace OnDash.Mathematics.Matrix;

public struct Matrix3 : IEquatable<Matrix3>
{
    private Vector3 _row0;

    public Vector3 Row0
    {
        get => _row0;
        set => _row0 = value;
    }

    private Vector3 _row1;

    public Vector3 Row1
    {
        get => _row1;
        set => _row1 = value;
    }

    private Vector3 _row2;

    public Vector3 Row2
    {
        get => _row2;
        set => _row2 = value;
    }

    public Matrix3(Vector3 row0, Vector3 row1, Vector3 row2)
    {
        _row0 = row0;
        _row1 = row1;
        _row2 = row2;
    }

    public Matrix3(
        float m00, float m01, float m02,
        float m10, float m11, float m12,
        float m20, float m21, float m22)
    {
        _row0 = new Vector3(m00, m01, m02);
        _row1 = new Vector3(m10, m11, m12);
        _row2 = new Vector3(m20, m21, m22);
    }

    public static Matrix3 Identity => new(Vector3.UnitX, Vector3.UnitY, Vector3.UnitZ);
    public static Matrix3 Zero => new(Vector3.Zero, Vector3.Zero, Vector3.Zero);

    public Vector3 Column0
    {
        get => new(M11, M21, M31);
        set
        {
            M11 = value.X;
            M21 = value.Y;
            M31 = value.Z;
        }
    }

    public Vector3 Column1
    {
        get => new(M12, M22, M32);
        set
        {
            M12 = value.X;
            M22 = value.Y;
            M32 = value.Z;
        }
    }

    public Vector3 Column2
    {
        get => new(M13, M23, M33);
        set
        {
            M13 = value.X;
            M23 = value.Y;
            M33 = value.Z;
        }
    }

    public float M11
    {
        get => _row0.X;
        set => _row0.X = value;
    }

    public float M12
    {
        get => _row0.Y;
        set => _row0.Y = value;
    }

    public float M13
    {
        get => _row0.Z;
        set => _row0.Z = value;
    }

    public float M21
    {
        get => _row1.X;
        set => _row1.X = value;
    }

    public float M22
    {
        get => _row1.Y;
        set => _row1.Y = value;
    }

    public float M23
    {
        get => _row1.Z;
        set => _row1.Z = value;
    }

    public float M31
    {
        get => _row2.X;
        set => _row2.X = value;
    }

    public float M32
    {
        get => _row2.Y;
        set => _row2.Y = value;
    }

    public float M33
    {
        get => _row2.Z;
        set => _row2.Z = value;
    }

    public float Trace => M11 + M22 + M33;

    public float Determinant => M11 * M22 * M33 + M12 * M23 * M31 +
                                M13 * M21 * M32 - M13 * M22 * M31 -
                                M11 * M23 * M32 - M12 * M21 * M33;

    public Vector3 Diagonal
    {
        get => new(M11, M22, M33);
        set
        {
            M11 = value.X;
            M22 = value.Y;
            M33 = value.Z;
        }
    }

    public float this[int rowIndex, int columnIndex]
    {
        get
        {
            return rowIndex switch
            {
                0 => _row0[columnIndex],
                1 => _row1[columnIndex],
                2 => _row2[columnIndex],
                _ => throw new IndexOutOfRangeException(
                    $"You tried to access this matrix at: ({rowIndex}, {columnIndex})")
            };
        }
        set
        {
            switch (rowIndex)
            {
                case 0:
                    _row0[columnIndex] = value;
                    break;
                case 1:
                    _row1[columnIndex] = value;
                    break;
                case 2:
                    _row2[columnIndex] = value;
                    break;
                default:
                    throw new IndexOutOfRangeException(
                        $"You tried to access this matrix at: ({rowIndex}, {columnIndex})");
            }
        }
    }

    public Matrix3 Transpose() => Transpose(this);

    public static Matrix3 Transpose(Matrix3 matrix)
    {
        return new Matrix3(matrix.Column0, matrix.Column1, matrix.Column2);
    }

    public Matrix3 Invert() => Invert(this);

    public static Matrix3 Invert(Matrix3 matrix)
    {
        var determinant = matrix.Determinant;

        if (determinant == 0.0f)
            throw new InvalidOperationException("The matrix is not invertible.");

        var inverseDeterminant = 1.0f / determinant;

        return new Matrix3
        (
            matrix.M33 * inverseDeterminant, -matrix.M23 * inverseDeterminant, matrix.M13 * inverseDeterminant,
            -matrix.M32 * inverseDeterminant, matrix.M22 * inverseDeterminant, -matrix.M12 * inverseDeterminant,
            matrix.M31 * inverseDeterminant, -matrix.M21 * inverseDeterminant, matrix.M11 * inverseDeterminant
        );
    }

    public Matrix3 Normalized()
    {
        return new Matrix3
        (
            Row0 / Determinant,
            Row1 / Determinant,
            Row2 / Determinant
        );
    }

    public Matrix3 Inverted()
    {
        var m = this;
        if (m.Determinant != 0) m.Invert();

        return m;
    }

    public static Matrix3 Add(Matrix3 left, Matrix3 right)
    {
        return new Matrix3
        (
            left.M11 + right.M11, left.M12 + right.M12, left.M13 + right.M13,
            left.M21 + right.M12, left.M22 + right.M22, left.M23 + right.M23,
            left.M31 + right.M31, left.M32 + right.M32, left.M33 + right.M33
        );
    }

    public static Matrix3 Multiply(Matrix3 left, Matrix3 right)
    {
        return new Matrix3
        (
            left.M11 * right.M11 + left.M12 * right.M21 + left.M13 * right.M31,
            left.M11 * right.M12 + left.M12 * right.M22 + left.M13 * right.M32,
            left.M11 * right.M13 + left.M12 * right.M23 + left.M13 * right.M33,
            left.M21 * right.M11 + left.M22 * right.M21 + left.M23 * right.M31,
            left.M21 * right.M12 + left.M22 * right.M22 + left.M23 * right.M32,
            left.M21 * right.M13 + left.M22 * right.M23 + left.M23 * right.M33,
            left.M31 * right.M11 + left.M32 * right.M21 + left.M33 * right.M31,
            left.M31 * right.M12 + left.M32 * right.M22 + left.M33 * right.M32,
            left.M31 * right.M13 + left.M32 * right.M23 + left.M33 * right.M33
        );
    }

    public Matrix3 ClearScale()
    {
        var matrix = this;
        return new Matrix3
        (
            matrix.Row0.Normalized(),
            matrix.Row1.Normalized(),
            matrix.Row2.Normalized()
        );
    }

    public Matrix3 ClearRotation()
    {
        var matrix = this;
        return new Matrix3
        (
            new Vector3(matrix.Row0.Length, 0, 0),
            new Vector3(0, matrix.Row1.Length, 0),
            new Vector3(0, 0, matrix.Row2.Length)
        );
    }

    public Vector3 ExtractScale() => new(Row0.Length, Row1.Length, Row2.Length);

    public Quaternion ExtractRotation(bool rowNormalize = true)
    {
        var row0 = Row0;
        var row1 = Row1;
        var row2 = Row2;
        if (!rowNormalize)
        {
            row0 = Row0.Normalized();
            row1 = Row1.Normalized();
            row2 = Row2.Normalized();
        }

        Quaternion quaternion;
        var trace = 0.25f * (row0[0] + row1[1] + row2[2] + 1);

        if (trace > 0)
        {
            var sq = MathHelper.Sqrt(trace);
            quaternion = new Quaternion
            (
                (row1[2] - row2[1]) / (4 * sq),
                (row2[0] - row0[2]) / (4 * sq),
                (row0[1] - row1[0]) / (4 * sq),
                sq
            );
        }
        else if (row0[0] > row1[1] && row0[0] > row2[2])
        {
            var sq = 2.0f * MathHelper.Sqrt(1f + row0[0] - row1[1] - row2[2]);

            quaternion = new Quaternion
            (
                0.25f * sq,
                (row1[0] + row0[1]) / sq,
                (row2[0] + row0[2]) / sq,
                (row2[1] - row1[2]) / sq
            );
        }
        else if (row1[1] > row2[2])
        {
            var sq = 2f * MathHelper.Sqrt(1f + row1[1] - row0[0] - row2[2]);

            quaternion = new Quaternion
            (
                (row1[0] + row0[1]) / sq,
                0.25f * sq,
                (row2[1] + row1[2]) / sq,
                (row2[0] - row0[2]) / sq
            );
        }
        else
        {
            var sq = 2.0f * MathHelper.Sqrt(1f + row2[2] - row0[0] - row1[1]);

            quaternion = new Quaternion
            (
                (row2[0] + row0[2]) / sq,
                (row2[1] + row1[2]) / sq,
                0.25f * sq,
                (row1[0] - row0[1]) / sq
            );
        }

        quaternion.Normalize();
        return quaternion;
    }

    public static Matrix3 CreateFromAxisAngle(Vector3 axis, float angle)
    {
        axis.Normalize();

        var cos = MathHelper.Cos(-angle);
        var sin = MathHelper.Sin(-angle);
        var t = 1f - cos;


        return new Matrix3
        (
            t * axis.X * axis.X + cos,
            t * axis.X * axis.Y - sin * axis.Z,
            t * axis.X * axis.Z + sin * axis.Y,
            t * axis.X * axis.Y + sin * axis.Z,
            t * axis.Y * axis.Y + cos,
            t * axis.Y * axis.Z - sin * axis.X,
            t * axis.X * axis.Z - sin * axis.Y,
            t * axis.Y * axis.Z + sin * axis.X,
            t * axis.Z * axis.Z + cos
        );
    }

    public static Matrix3 CreateFromQuaternion(Quaternion quaternion)
    {
        quaternion.ToAxisAngle(out var axis, out var angle);
        return CreateFromAxisAngle(axis, angle);
    }

    public static Matrix3 CreateRotationX(float angle)
    {
        var cos = MathHelper.Cos(angle);
        var sin = MathHelper.Sin(angle);

        var result = Identity;
        result.M22 = cos;
        result.M23 = sin;
        result.M32 = -sin;
        result.M33 = cos;

        return new Matrix3
        (
            1, 0, 0,
            0, cos, sin,
            0, -sin, cos
        );
    }

    public static Matrix3 CreateRotationY(float angle)
    {
        var cos = MathHelper.Cos(angle);
        var sin = MathHelper.Sin(angle);

        return new Matrix3
        (
            cos, 0, -sin,
            0, 1, 0,
            sin, 0, cos
        );
    }

    public static Matrix3 CreateRotationZ(float angle)
    {
        var cos = MathHelper.Cos(angle);
        var sin = MathHelper.Sin(angle);

        return new Matrix3(
            cos, sin, 0,
            -sin, cos, 0,
            0, 0, 1
        );
    }

    public static Matrix3 CreateScale(float scale) => CreateScale(scale, scale, scale);
    public static Matrix3 CreateScale(Vector3 scale) => CreateScale(scale.X, scale.Y, scale.Z);

    public static Matrix3 CreateScale(float x, float y, float z)
    {
        return new Matrix3
        (
            x, 0, 0,
            0, y, 0,
            0, 0, z
        );
    }

    public static Matrix3 operator +(Matrix3 a, Matrix3 b) => Add(a, b);
    public static Matrix3 operator *(Matrix3 a, Matrix3 b) => Multiply(a, b);

    public static bool operator ==(Matrix3 a, Matrix3 b) => a.Equals(b);
    public static bool operator !=(Matrix3 a, Matrix3 b) => !a.Equals(b);

    public override string ToString() => $"{Row0}\n{Row1}\n{Row2}";

    public override int GetHashCode() => HashCode.Combine(_row0, _row1, _row2);


    public override bool Equals(object? obj)
        => obj is Matrix3 other && Equals(other);

    public bool Equals(Matrix3 other)
    {
        return _row0.Equals(other._row0) &&
               _row1.Equals(other._row1) &&
               _row2.Equals(other._row2);
    }
}